/*
 * communicate_uart.c
 *
 *  Created on: 2022.2.26
 *      Author: huan
 */
#include "head_user.h"
#include "head_rtthread.h"

/*-------------------------------------------------------------------------------------------------*/
rt_mutex_t Vcom = RT_NULL;
/*i ANO potocol analyse i*/
void vcom_rxTask(void* parameter)
{
    size_t  size_dat = 0;
    char    dat_p[64] = {0};
    char*   dat = 0;
    Data_packge_t* Ano_packge = RT_NULL;
    while(1)
    {
        size_dat = USER_VCOM.recv(dat_p, RT_WAITING_FOREVER);
        dat = dat_p;
        for (uint8_t i = 0; i < size_dat; i++)
        {
            Ano_packge = ANO_Send.ANO_Data_Receive(*(dat++));
        }
        if((rt_ubase_t)Ano_packge)
        {
            /*i 读写线程均用到了vcom发送，互斥访问 i*/
            rt_mutex_take(Vcom, RT_WAITING_FOREVER);

            ANO_Send.Data_analyze(Ano_packge->buffer, *(Ano_packge->len));

            rt_mutex_release(Vcom);
        }
    }
}
void vcom_txTask(void* parameter)
{
    imuData_t* imu = IMU_Handle.imu_data;
    IMU_st*    imu_data = INS_Handle.imuAngle;
    uint16_t*  ppm      = RC_Handle.rc_ori;
    PID_Struct *pid = PID_Handle.pid;
    while(1)
    {
        /*i 读写线程均用到了vcom发送，互斥访问 i*/
        rt_mutex_take(Vcom, RT_WAITING_FOREVER);

        //ANO_Send.Inertial(acce_raw_adc.x,acce_raw_adc.y,acce_raw_adc.z,gyro_raw_adc.x,gyro_raw_adc.y,gyro_raw_adc.z);   //原始ADC
        //ANO_Send.Inertial(acce_ripe_adc.x,acce_ripe_adc.y,acce_ripe_adc.z,gyro_ripe_adc.x,gyro_ripe_adc.y,gyro_ripe_adc.z);
        //ANO_Send.Inertial(Encoder_Handle[enco_motum].angular_V,Encoder_Handle[enco_motum].angular_A,imu.acce.z,imu.gyro.x,imu.gyro.y,imu.gyro.z);
        //ANO_Send.Inertial(pid[Pit_Rate].OutPut,pid[Yaw_Rate].OutPut,pid[Rol_Rate].OutPut,(*imu).gyro.x,(*imu).gyro.y,(*imu).gyro.z);
        //ANO_Send.Inertial(pid[Rol_Angle].Kp*1000,pid[Rol_Angle].Ki*1000,pid[Rol_Angle].Kd*1000,imu.gyro.x,imu.gyro.y,imu.gyro.z);
        //ANO_Send.CpsBarTem(3, 4, 5 ,6*100 ,7*100);
        //ANO_Send.CpsBarTem(Core0_data.Core1M.Uq*100, pid[Mot_Speed].FeedBack,Core0_data.Core0M.speed,6*100 ,7*100);
        ANO_Send.EulerAngle((*imu_data).roll*100,(*imu_data).pitch*100,(*imu_data).yaw*100);
        ANO_Send.RemoteData(ppm[0],ppm[1],ppm[2],ppm[3],ppm[4],ppm[5],ppm[6],ppm[7],ppm[8],ppm[9]);
        ANO_Send.Quaternions((*imu_data).q0*10000, (*imu_data).q1*10000, (*imu_data).q2*10000, (*imu_data).q3*10000);

        /* 机体系与导航系的惯性数据 cm/s^2 */
        //ANO_Send.Inertial(pid[Z_Speed].OutPut, pid[Z_Speed].Kp*100, BODY.Pos.z, *Sensor_Handle.height, pid[Z_Position].Kp*100, BODY.Speed.z);
        //ANO_Send.Inertial(BODY.Pos.x, BODY.Pos.y, BODY.Pos.z, BODY.PosObser.x, BODY.PosObser.y, BODY.PosObser.z);
        //ANO_Send.Inertial(Sensor_Handle.tofheight, pid[Yaw_Angle].OutPut, BODY.nfPos.z, *Sensor_Handle.height, BODY.nfSpeed.y, BODY.nfSpeed.z);
        //ANO_Send.Inertial(BODY.Pos.z, BODY.Speed.z, BODY.Acce.z, BODY.nfAcce.x, BODY.nfAcce.y, BODY.nfAcce.z);
        //ANO_Send.Inertial(imu->acce.x, imu->acce.y, imu->acce.z, (*imu).gyro.x,(*imu).gyro.y,(*imu).gyro.z);
        //ANO_Send.Inertial(BODY.PosObser.x*100, BODY.PosObser.y*100, BODY.PosObser.z*100, BODY.VelObser.x, BODY.VelObser.y, BODY.VelObser.z);
        ANO_Send.Inertial(BODY.bfSpeed.x, BODY.bfSpeed.y, BODY.bfSpeed.z, BODY.nfSpeed.x, BODY.nfSpeed.y, BODY.nfSpeed.z);

        rt_mutex_release(Vcom);
        rt_thread_mdelay(10);
    }
}
/*-------------------------------------------------------------------------------------------------*/

/*-------------------------------------------------------------------------------------------------*/
void uart1_rxTask(void* parameter)
{
    size_t  size_dat = 0;
    char    dat_p[64] = {0};
    char*   dat = 0;
    Data_packge_t* Ano_packge = RT_NULL;
    while(1)
    {
        size_dat = USER_UART1.recv(dat_p, RT_WAITING_FOREVER);
        dat = dat_p;
        for (uint8_t i = 0; i < size_dat; i++)
        {
            Ano_packge = ANO_Send.ANO_Data_Receive(*(dat++));
        }
        if((rt_ubase_t)Ano_packge)
        {
            ANO_Send.Data_analyze(Ano_packge->buffer, *(Ano_packge->len));
        }
    }
}
void uart1_txTask(void* parameter)
{
    imuData_t* imu = IMU_Handle.imu_data;
    IMU_st*    imu_data = INS_Handle.imuAngle;
    uint16_t*  ppm      = RC_Handle.rc_ori;
    PID_Struct *pid = PID_Handle.pid;

    AUTO_Order* AUTO = &Task_Handle.CtrlSource.AUTO;
    while(1)
    {
        ANO_Send.EulerAngle((*imu_data).roll*100,(*imu_data).pitch*100,(*imu_data).yaw*100);
        ANO_Send.RemoteData(ppm[0],ppm[1],ppm[2],ppm[3],ppm[4],ppm[5],ppm[6],ppm[7],ppm[8],ppm[9]);
        //ANO_Send.Quaternions((*imu_data).q0*10000, (*imu_data).q1*10000, (*imu_data).q2*10000, (*imu_data).q3*10000);
        //ANO_Send.Inertial(Sensor_Handle.tofheight, pid[Yaw_Angle].OutPut, BODY.nfPos.z, *Sensor_Handle.height, BODY.nfSpeed.y, BODY.nfSpeed.z);
        //ANO_Send.Inertial(BODY.nfPos.x, BODY.nfPos.y, BODY.nfPos.z, BODY.nfSpeed.x, BODY.nfSpeed.y, BODY.nfSpeed.z);
        //ANO_Send.Inertial(BODY.nfPos.x, BODY.nfPos.y, pid[X_Position].Expect, Sensor_Handle.tofheight, BODY.PosR.y, (*imu).gyro.z);
        ANO_Send.Inertial(BODY.nfPos.x, BODY.nfPos.y, *Sensor_Handle.height, pid[X_Position].Expect, pid[Y_Position].Expect, pid[Z_Position].Expect);
        //ANO_Send.CpsBarTem(BODY.bfSpeed.x, BODY.bfSpeed.y, pid[X_Position].Err ,6*100 ,7*100);
        ANO_Send.CpsBarTem(Debug_Handle.val[0], Debug_Handle.val[1], Debug_Handle.val[2],BODY.nfPos.z*100 ,7*100);
        //ANO_Send.Inertial(BODY.nfAcce.x, BODY.nfAcce.y, BODY.nfAcce.z, BODY.bfAcce.x, BODY.bfAcce.y, BODY.bfAcce.z);
        //ANO_Send.Inertial(imu->acce.x, imu->acce.y, imu->acce.z, (*imu).gyro.x,(*imu).gyro.y,(*imu).gyro.z);
        rt_thread_mdelay(10);
    }
}
/*-------------------------------------------------------------------------------------------------*/





/*-------------------------------------------------------------------------------------------------*/
static void TOF_DataAnalyze(uint8_t *buf)
{
    Sensor_Handle.tofheight = (float)((uint32_t)(buf[8] | buf[9]<<8 | buf[10]<<16))/10.0f;
}
void uart3_rxTask(void* parameter)
{
    uint8_t  size_dat = 0;
    char    dat_p[64] = {0};
    while(1)
    {
        size_dat = USER_UART3.recv(dat_p, RT_WAITING_FOREVER);
        if(size_dat == 16 && dat_p[0] == 0x57)
        {
            TOF_DataAnalyze((uint8_t*)dat_p);
        }
    }
}
void uart3_txTask(void* parameter)
{
    while(1)
    {
        rt_thread_mdelay(1000);
    }
}
/*-------------------------------------------------------------------------------------------------*/


/*-------------------------------------------------------------------------------------------------*/
static void UWB_DataAnalyze(uint8_t *buf)
{
    BODY.PosObser.x = (int32_t)(buf[ 4]<<8 | buf[ 5]<<16 | buf[ 6]<<24)/256/10.0f;
    BODY.PosObser.y = (int32_t)(buf[ 7]<<8 | buf[ 8]<<16 | buf[ 9]<<24)/256/10.0f;
    BODY.PosObser.z = -(int32_t)(buf[10]<<8 | buf[11]<<16 | buf[12]<<24)/256/10.0f;

    BODY.VelObser.x = (int32_t)(buf[13]<<8 | buf[14]<<16 | buf[15]<<24)/256/100.0f;
    BODY.VelObser.y = (int32_t)(buf[16]<<8 | buf[17]<<16 | buf[18]<<24)/256/100.0f;
    BODY.VelObser.z = -(int32_t)(buf[19]<<8 | buf[20]<<16 | buf[21]<<24)/256/100.0f;


    if(buf[117] == 0xff){
        BODY.PosR.x = 10000.0f;
    }
    else {
        BODY.PosR.x = buf[117]/1.0f;
    }
    if(buf[118] == 0xff){
        BODY.PosR.y = 10000.0f;
    }
    else {
        BODY.PosR.y = buf[118]/1.0f;
    }
    if(buf[119] == 0xff){
        BODY.PosR.z = 10000.0f;
    }
    else {
        BODY.PosR.z = buf[119]/1.0f;
    }
}

void uart4_rxTask(void* parameter)
{
    size_t  size_dat = 0;
    char    dat_p[200] = {0};
    while(1)
    {
        size_dat = USER_UART4.recv(dat_p, RT_WAITING_FOREVER);
        if(size_dat == 128)
        {
            UWB_DataAnalyze((uint8_t*)dat_p);
        }
    }
}
void uart4_txTask(void* parameter)
{
    while(1)
    {
        //USER_UART4.send("uart4 is sending!\r\n",sizeof("uart4 is sending!\r\n"));
        rt_thread_mdelay(1000);
    }
}
/*-------------------------------------------------------------------------------------------------*/


/*-------------------------------------------------------------------------------------------------*/
static void Sbus_Data_Count(uint8_t *buf)
{
    RC_Handle.rc_raw[ 0] = ((int16_t)buf[ 2] >> 0 | ((int16_t)buf[ 3] << 8 )) & 0x07FF;
    RC_Handle.rc_raw[ 1] = ((int16_t)buf[ 3] >> 3 | ((int16_t)buf[ 4] << 5 )) & 0x07FF;
    RC_Handle.rc_raw[ 2] = ((int16_t)buf[ 4] >> 6 | ((int16_t)buf[ 5] << 2 )  | (int16_t)buf[ 6] << 10 ) & 0x07FF;
    RC_Handle.rc_raw[ 3] = ((int16_t)buf[ 6] >> 1 | ((int16_t)buf[ 7] << 7 )) & 0x07FF;
    RC_Handle.rc_raw[ 4] = ((int16_t)buf[ 7] >> 4 | ((int16_t)buf[ 8] << 4 )) & 0x07FF;
    RC_Handle.rc_raw[ 5] = ((int16_t)buf[ 8] >> 7 | ((int16_t)buf[ 9] << 1 )  | (int16_t)buf[10] <<  9 ) & 0x07FF;
    RC_Handle.rc_raw[ 6] = ((int16_t)buf[10] >> 2 | ((int16_t)buf[11] << 6 )) & 0x07FF;
    RC_Handle.rc_raw[ 7] = ((int16_t)buf[11] >> 5 | ((int16_t)buf[12] << 3 )) & 0x07FF;
    RC_Handle.rc_raw[ 8] = ((int16_t)buf[13] << 0 | ((int16_t)buf[14] << 8 )) & 0x07FF;
    RC_Handle.rc_raw[ 9] = ((int16_t)buf[14] >> 3 | ((int16_t)buf[15] << 5 )) & 0x07FF;

//    RC_Handle.rc_raw[10] = ((int16_t)buf[15] >> 6 | ((int16_t)buf[16] << 2 )  | (int16_t)buf[17] << 10 ) & 0x07FF;
//    RC_Handle.rc_raw[11] = ((int16_t)buf[17] >> 1 | ((int16_t)buf[18] << 7 )) & 0x07FF;
//    RC_Handle.rc_raw[12] = ((int16_t)buf[18] >> 4 | ((int16_t)buf[19] << 4 )) & 0x07FF;
//    RC_Handle.rc_raw[13] = ((int16_t)buf[19] >> 7 | ((int16_t)buf[20] << 1 )  | (int16_t)buf[21] <<  9 ) & 0x07FF;
//    RC_Handle.rc_raw[14] = ((int16_t)buf[21] >> 2 | ((int16_t)buf[22] << 6 )) & 0x07FF;
//    RC_Handle.rc_raw[15] = ((int16_t)buf[22] >> 5 | ((int16_t)buf[23] << 3 )) & 0x07FF;
}

/* RC input analyze */
void uart6_rxTask(void* parameter)
{
    size_t      size_dat = 0;
    uint8_t     dat_p[64] = {0};
    while(1)
    {
        size_dat = USER_UART6.recv((char*)dat_p, RT_WAITING_FOREVER);
        if( size_dat == 25 )
        {
            Sbus_Data_Count(dat_p-1);
        }
    }
}
/*-------------------------------------------------------------------------------------------------*/

static int communicate_init(void)
{
    Vcom = rt_mutex_create("Vcom", RT_IPC_FLAG_FIFO);

    return 0;
}

INIT_APP_EXPORT(communicate_init);
/*-------------------------------------------------------------------------------------------------*/
